// 2025.8.22 青浦演示视觉（去除力控版）
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <atomic>
#include <thread>
#include <rclcpp/executors/multi_threaded_executor.hpp>

#include <algorithm>
#include <vector>
#include "path_plan/path1_waypoints.hpp"

#define USE_IBVS  // 定义宏以启用 IBVS 功能

#ifdef USE_IBVS
#include "/home/jetson/zc6/src/servo_visual/include/servo_visual/ibvs_control.hpp"
#else
#include "/home/jetson/zc6/src/servo_visual/include/servo_visual/visual_control.hpp"
#endif
using servo_visual::IBVSControl; 
class CombinedNode : public rclcpp::Node {
public:
    CombinedNode() : Node("combined_node") {
        // 初始化 MoveGroupInterface（保持你原本的写法）
        move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
            std::shared_ptr<rclcpp::Node>(this),  // 显式创建shared_ptr（注意：保持你的原写法）
            "zc4");

        RCLCPP_INFO(get_logger(), "节点初始化完成");
    }

#ifdef USE_IBVS
    std::shared_ptr<servo_visual::IBVSControl> get_ibvs_control_node() {
        return ibvs_control_node;
    }
#else
    std::shared_ptr<VisualControl> get_visual_control_node() {
        return visual_control_node_;
    }
#endif

    // 接收路径索引并执行
    void start_execution(int path_index) {
        std::thread([this, path_index]() {
            switch (path_index) {
                case 1:
                    // 1) 走笛卡尔路径
                    if (!executeCartesianPath1()) {
                        RCLCPP_ERROR(get_logger(), "笛卡尔路径执行失败");
                        rclcpp::shutdown();
                        return;
                    }

                    // 2) 视觉阶段
#ifdef USE_IBVS
                    ibvs_control_node = std::make_shared<IBVSControl>();
                    ibvs_control_node->initialize();
                    while (rclcpp::ok() && !ibvs_control_node->isFinished()) {
                        RCLCPP_INFO(get_logger(), "IBVS 执行中...");
                        std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    }
                    RCLCPP_INFO(get_logger(), "IBVS 执行完毕，开始返回 middle 位置");
#else
                    visual_control_node_ = std::make_shared<VisualControl>(/* 可传入必要参数 */);
                    visual_control_node_->initialize();
                    while (rclcpp::ok() && !visual_control_node_->isFinished()) {
                        RCLCPP_INFO(get_logger(), "Visual 执行中...");
                        std::this_thread::sleep_for(std::chrono::milliseconds(100));
                    }
                    RCLCPP_INFO(get_logger(), "Visual 执行完毕，开始返回 middle 位置");
#endif

                    // 3) 回到 middle
                    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
                    if (returnToMiddle()) {
                        RCLCPP_INFO(get_logger(), "成功返回 middle 位置");
                    } else {
                        RCLCPP_ERROR(get_logger(), "返回 middle 位置失败");
                    }
                    rclcpp::shutdown();
                    break;

                default:
                    RCLCPP_ERROR(get_logger(), "无效的路径索引: %d", path_index);
                    break;
            }
        }).detach();
    }

private:
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
#ifdef USE_IBVS
    std::shared_ptr<servo_visual::IBVSControl> ibvs_control_node;
#else
    std::shared_ptr<VisualControl> visual_control_node_;
#endif

    bool returnToMiddle() {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000)); // 等待MoveGroup就绪
        move_group_->setNamedTarget("middle");
        moveit::planning_interface::MoveGroupInterface::Plan home_plan;
        bool success = (move_group_->plan(home_plan) == moveit::core::MoveItErrorCode::SUCCESS);
        if (success) {
            auto exec_ret = move_group_->execute(home_plan);
            if (exec_ret == moveit::core::MoveItErrorCode::SUCCESS) {
                RCLCPP_INFO(get_logger(), "回到middle位置");
                return true;
            }
        }
        RCLCPP_ERROR(get_logger(), "回到middle位置失败");
        return false;
    }

    // 仅执行一次完整笛卡尔路径（不再因力控停/续）
    bool executeCartesianPath1() {
        if (!returnToMiddle()) return false;

        // 读取外部文件定义的路径
        std::vector<geometry_msgs::msg::Pose> waypoints = paths::path1_waypoints();

        move_group_->stop();
        move_group_->setStartStateToCurrentState();
        moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
        double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);

        if (fraction < 0.9) {
            RCLCPP_ERROR(get_logger(), "初始路径规划失败，完成度: %f", fraction);
            return false;
        }

        RCLCPP_INFO(get_logger(), "初始路径规划成功，开始执行...");
        auto exec_ret = move_group_->execute(cartesian_plan);
        if (exec_ret != moveit::core::MoveItErrorCode::SUCCESS) {
            RCLCPP_ERROR(get_logger(), "笛卡尔路径执行失败");
            move_group_->stop();
            return false;
        }

        RCLCPP_INFO(get_logger(), "笛卡尔路径执行完成");
        move_group_->stop();
        return true;
    }
};

int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<CombinedNode>();

    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);

    node->start_execution(1);

#ifdef USE_IBVS
    // 等待 IBVSControl 节点创建成功
    while (rclcpp::ok() && node->get_ibvs_control_node() == nullptr) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    executor.add_node(node->get_ibvs_control_node());
#else
    // 等待 VisualControl 节点创建成功
    while (rclcpp::ok() && node->get_visual_control_node() == nullptr) {
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
    executor.add_node(node->get_visual_control_node());
#endif

    RCLCPP_INFO(node->get_logger(), "开始执行器 spin");
    executor.spin();

    RCLCPP_INFO(node->get_logger(), "执行器 spin 结束");
    rclcpp::shutdown();
    return 0;
}
